/*
 * ModelReconstructor.cpp
 *
 *  Created on: Apr 14, 2009
 *      Author: wrice
 */

#include "MyVideoTrace.h"
#include <math.h>
#include <fstream>
namespace MyVideoTrace {


struct ModelReconstructor::Impl {

	class FindFeaturePointWithPointerFunctor {
	public:
		FindFeaturePointWithPointerFunctor( FeaturePoint * ptr )
		: ptr_(ptr)
		{}

		bool operator()(const FeaturePointPtr featurePoint) const {
			return FeaturePointPtr::GetPointer( featurePoint ) == ptr_;
		}
	private:
		FeaturePoint * ptr_;
	};

	class FindFeaturePointWithPoint2DFunctor {
	public:
		FindFeaturePointWithPoint2DFunctor(FrameNumber frameNumber, Point2D userPoint)
		: frameNumber_(frameNumber), userPoint_(userPoint) {}

		bool operator()(const FeaturePointPtr featurePoint) const {
			FeaturePoint2D point2d(featurePoint, frameNumber_);
			if( false == point2d.isExist() ) return false;

			return ( point2d.getPoint() == userPoint_ );
		}
	private:
		const FrameNumber frameNumber_;
		const Point2D userPoint_;
	};


	// -------- find near --------------------


	template< typename Iterator, typename Func >
	static Func for_each_with_iterator(Iterator begin, Iterator end, Func & func) {
		for(Iterator iter = begin; iter != end; ++iter)
			func(iter);
		return func;
	}


	template< typename Iterator, typename DistanceType >
	class NearestIterator {
	public:
		NearestIterator(Iterator iterEnd) : found_(false), iterNearest_(iterEnd) {}

		Iterator getTheNearestIterator() const { return iterNearest_; }

		void setTheNearestIteratorIfNearer(Iterator iter, float distance) {
			if(false == isNearer(distance) ) return;
			found_ = true;
			minDistance_ = distance;
			iterNearest_ = iter;
		}

	private:
		bool isNearer(DistanceType distance) const {
			return false == found_ || distance < minDistance_;
		}

		bool found_;
		DistanceType minDistance_;
		Iterator iterNearest_;
	};


	template< typename Iterator >
	class NearFindFeaturePointFunctor {
	public:
		NearFindFeaturePointFunctor(VoodooCameraInfo * camera, FrameNumber frameNumber,
				Point2D userPoint, float withinDistance, Iterator iterEnd)
		: camera_(camera), frameNumber_(frameNumber), userPoint_(userPoint)
		, withinDistance_(withinDistance), nearestIterator_(iterEnd)
		{}

		void operator()(Iterator iter) {
			FeaturePoint2D point2d(*iter, frameNumber_);
			Point2DPtr point = NULL;
			if( point2d.isExist() )
				point = new Point2D( point2d.getPoint() );
			else if( NULL != camera_ )
				point = new Point2D( getProjectionPoint( *camera_, (*iter)->getPoint3D() ) );
			else return;

			const float distance = getDistanceSquared( *point );
			if( distance <= withinDistance_ * withinDistance_ )
					nearestIterator_.setTheNearestIteratorIfNearer(iter, distance);
		}

		Iterator getTheNearestIterator() const { return nearestIterator_.getTheNearestIterator(); }

	private:
		float getDistanceSquared(Point2D candidatePoint) const {
			const Point2D dist = candidatePoint - userPoint_;
			return dist.getX() * dist.getX() + dist.getY() * dist.getY();
		}

		VoodooCameraInfo * camera_;
		const FrameNumber frameNumber_;
		const Point2D userPoint_;
		const float withinDistance_;
		NearestIterator< Iterator, float > nearestIterator_;
	};

	static string startingOfX3D;
	static string endingOfX3D;
};

ModelReconstructor::ModelReconstructor() {
}

size_t ModelReconstructor::sizeFeaturePoints() const {
	return featurePoints_.size();
}

bool ModelReconstructor::hasFeaturePoint(FrameNumber whichFrame, Point2D point) const {
	return NULL != findFeaturePoint( whichFrame, point );
}

FeaturePoint * ModelReconstructor::findFeaturePoint(FrameNumber whichFrame, Point2D point) const {
	FeaturePointsIterator iter = find_if(featurePoints_.begin(), featurePoints_.end(), Impl::FindFeaturePointWithPoint2DFunctor(whichFrame, point) );
	if( iter == featurePoints_.end() ) return NULL;
	return FeaturePointPtr::GetPointer(*iter);
}

FeaturePoint * ModelReconstructor::findNearFeaturePoint(FrameNumber whichFrame, Point2D point, float distance) const {
	VoodooCameraInfoPtr camera = NULL;
	if( voodooLoader_->getNumberOfCameraInfo() > whichFrame )
		camera = new VoodooCameraInfo(voodooLoader_->getCameraInfo( whichFrame ));

	Impl::NearFindFeaturePointFunctor<FeaturePointsIterator>
		func(VoodooCameraInfoPtr::GetPointer(camera), whichFrame, point, distance, featurePoints_.end() );

	FeaturePointsIterator iter = Impl::for_each_with_iterator(featurePoints_.begin(), featurePoints_.end(), func).getTheNearestIterator();
	if( iter == featurePoints_.end() ) return NULL;
	return FeaturePointPtr::GetPointer(*iter);
}

// ---------- feature lines --------------

ModelReconstructor::FeaturePointsModifiableIterator ModelReconstructor::findFeaturePointForModify(FrameNumber frameNumber, Point2D point) {
	return find_if(featurePoints_.begin(), featurePoints_.end(), Impl::FindFeaturePointWithPoint2DFunctor(frameNumber, point) );
}

// ----------- adding --------------

int ModelReconstructor::generateNewFeaturePointID() {
	int newId = 0;
	FeaturePointsIterator iter = featurePoints_.begin();
	for(; iter != featurePoints_.end(); ++iter)
		if( newId <= (*iter)->getFeaturePointID() )
			newId = (*iter)->getFeaturePointID() +1;
	return newId;
}

FeaturePoint * ModelReconstructor::addPoint2D(FrameNumber whichFrame, Point2D point) {
	if( hasFeaturePoint(whichFrame, point) ) return NULL;

	FeaturePoint * newPoint = new FeaturePoint( generateNewFeaturePointID() );
	newPoint->setPoint2D(whichFrame, point);
	featurePoints_.insert(newPoint);

	if( voodooLoader_ && whichFrame < voodooLoader_->getNumberOfCameraInfo() ) {
		FeaturePointPtr closestFeaturePoint = NULL;
		float minimumDistance = 0;
		for( size_t idx = 0; idx < voodooLoader_->getNumberOfFeaturePoints(); ++idx ) {
			FeaturePoint candidateFeaturePoint = voodooLoader_->getFeaturePoint( idx );
			FeaturePoint::Points2DIterator iterPoint2D = candidateFeaturePoint.findPoint2D( whichFrame );
			if( iterPoint2D == candidateFeaturePoint.endPoint2D() ) continue;

			const Point2D candidatePoint = iterPoint2D->second;
			const float distance = point.distance( candidatePoint );
			if( closestFeaturePoint == NULL || minimumDistance > distance ) {
				minimumDistance = distance;
				closestFeaturePoint = new FeaturePoint( candidateFeaturePoint );
			}
		}

		if( NULL != closestFeaturePoint ) {
			VoodooCameraInfo camera = voodooLoader_->getCameraInfo( whichFrame );

			Vector3f fpoint3D = closestFeaturePoint->getPoint3D();
			Vector3f posIn2D = getProjectionPosition( camera, fpoint3D );
			const float projZ = posIn2D.getZ();
			Vector3f projXYZ(
					(point.getX() - camera.getImageSizeWidth() /2 -1) * projZ,
					(point.getY() - camera.getImageSizeHeight() /2 -1) * projZ,
					projZ );

			Vector3f posIn3D = getRevertedPosition( camera, projXYZ );
			newPoint->setPoint3D( posIn3D );
		}
	}

	return newPoint;
}

bool ModelReconstructor::hasFeaturePoint( FeaturePoint * featurePoint ) const {
	if( NULL == featurePoint ) return false;
	FeaturePointsIterator iter = find_if( featurePoints_.begin(), featurePoints_.end(), Impl::FindFeaturePointWithPointerFunctor( featurePoint ) );
	return( iter != featurePoints_.end() );
}

bool ModelReconstructor::hasFeatureLine(FeaturePoint * lineFrom, FeaturePoint * lineTo) {
	FeatureLinesIterator iter = featureLines_.begin();
	for(; iter != featureLines_.end(); ++iter ) {
		if( iter->getLineFrom() == lineFrom && iter->getLineTo() == lineTo ) return true;
		if( iter->getLineTo() == lineFrom && iter->getLineFrom() == lineTo ) return true;
	}
	return false;
}

void ModelReconstructor::addLine3D( FeaturePoint * lineFrom, FeaturePoint * lineTo ) {
	if( NULL == lineFrom || NULL == lineTo ) return;
	if( false == hasFeaturePoint(lineFrom) ) return;
	if( false == hasFeaturePoint(lineTo) ) return;
	if( hasFeatureLine(lineFrom, lineTo) ) return;

	FeatureLine newLine( lineFrom, lineTo );
	featureLines_.insert(newLine);
}

void ModelReconstructor::importVoodoo( const VoodooLoader * voodooLoader ) {
	voodooLoader_ = voodooLoader;
}

void ModelReconstructor::moveFeaturePoint(FeaturePoint * fpoint, FrameNumber whichFrame, Point2D moveTo) {
	if( ! hasFeaturePoint( fpoint ) ) return;
	if( hasFeaturePoint( whichFrame, moveTo ) ) return;

	fpoint->removePoint2D(whichFrame);
	fpoint->setPoint2D( whichFrame, moveTo );
	adjust3DCoordination( fpoint );
}

void ModelReconstructor::adjust3DCoordination( FeaturePoint * featurePoint ) {
	list< Vector3f > candidatePoints;

	FeaturePoint::Points2DIterator iterPoint2D = featurePoint->beginPoint2D();
	for(; iterPoint2D != featurePoint->endPoint2D(); ++iterPoint2D ) {
		const FrameNumber eachFrame = iterPoint2D->first;
		const Point2D eachPoint2D = iterPoint2D->second;
		VoodooCameraInfo eachCamera = voodooLoader_->getCameraInfo( eachFrame );

		Vector3f fpoint3D = featurePoint->getPoint3D();
		Vector3f posIn2D = getProjectionPosition( eachCamera, fpoint3D );
		const float projZ = posIn2D.getZ();
		Vector3f projXYZ(
				(eachPoint2D.getX() - eachCamera.getImageSizeWidth() /2 -1) * projZ,
				(eachPoint2D.getY() - eachCamera.getImageSizeHeight() /2 -1) * projZ,
				projZ );

		Vector3f candidatePoint3D = getRevertedPosition( eachCamera, projXYZ );
		candidatePoints.push_back( candidatePoint3D );
	}

	Vector3f middlePoint(0,0,0);
	list< Vector3f >::const_iterator iterCandidate = candidatePoints.begin();
	for(; iterCandidate != candidatePoints.end(); ++iterCandidate )
		middlePoint = middlePoint + *iterCandidate;

	featurePoint->setPoint3D( middlePoint / candidatePoints.size() );
}

void ModelReconstructor::removeFeaturePoint( FeaturePoint * fpoint ) {
	FeatureLinesModifiableIterator iterLine = featureLines_.begin();
	for(; iterLine != featureLines_.end(); ) {
		if( iterLine->getLineFrom() == fpoint || iterLine->getLineTo() == fpoint ) {
			featureLines_.erase( iterLine++ );
		} else
			++iterLine;
	}

	FeaturePointsModifiableIterator iterPoint = featurePoints_.begin();
	for(; iterPoint != featurePoints_.end(); ++iterPoint ) {
		if( FeaturePointPtr::GetPointer(*iterPoint) == fpoint ) {
			featurePoints_.erase( iterPoint );
			return;
		}
	}
}


CvMat ModelReconstructor::getInvertMatrix( CvMat M ) {
	CvMat * Mi = cvCreateMat( M.cols, M.rows, M.type );
	cvInvert( &M, Mi );
	CvMat Mrst( *Mi );
	cvReleaseMat( &Mi );
	return Mrst;
}

Vector3f ModelReconstructor::getRevertedPosition( VoodooCameraInfo camera, Vector3f posIn2D ) {
	CvMat * M1 = cvCreateMat( 3, 3, CV_32FC1 );
	cvmSet( M1, 0, 0, camera.getCameraHorizonX() );
	cvmSet( M1, 0, 1, camera.getCameraHorizonY() );
	cvmSet( M1, 0, 2, camera.getCameraHorizonZ() );
	cvmSet( M1, 1, 0, camera.getCameraVerticalX() );
	cvmSet( M1, 1, 1, camera.getCameraVerticalY() );
	cvmSet( M1, 1, 2, camera.getCameraVerticalZ() );
	cvmSet( M1, 2, 0, camera.getCameraAxisX() );
	cvmSet( M1, 2, 1, camera.getCameraAxisY() );
	cvmSet( M1, 2, 2, camera.getCameraAxisZ() );

	CvMat * M1invert = cvCreateMat( 3, 3, CV_32FC1 );
	cvInvert( M1, M1invert );

	CvMat * Mlhs = cvCreateMat( 3, 1, CV_32FC1 );
	cvmSet( Mlhs, 0, 0, posIn2D.getX() );
	cvmSet( Mlhs, 1, 0, posIn2D.getY() );
	cvmSet( Mlhs, 2, 0, posIn2D.getZ() );

	CvMat * Mtmp1 = cvCreateMat( 3, 1, CV_32FC1 );
	cvMatMul( M1invert, Mlhs, Mtmp1 );

	CvMat * M2 = cvCreateMat( 3, 1, CV_32FC1 );
	cvmSet( M2, 0, 0, camera.getCameraPositionX() );
	cvmSet( M2, 1, 0, camera.getCameraPositionY() );
	cvmSet( M2, 2, 0, camera.getCameraPositionZ() );

	CvMat * Mtmp2 = cvCreateMat( 3, 1, CV_32FC1 );
	cvAdd( Mtmp1, M2, Mtmp2 );

	const float x = cvmGet( Mtmp2, 0, 0);
	const float y = cvmGet( Mtmp2, 1, 0);
	const float z = cvmGet( Mtmp2, 2, 0);

	cvReleaseMat( &M1 );
	cvReleaseMat( &M1invert );
	cvReleaseMat( &M2 );
	cvReleaseMat( &Mlhs );
	cvReleaseMat( &Mtmp1 );
	cvReleaseMat( &Mtmp2 );

	return Vector3f( x, y, z );
}

Vector3f ModelReconstructor::getProjectionPosition( VoodooCameraInfo camera, Vector3f posIn3D ) {
	CvMat * M1 = cvCreateMat( 3, 3, CV_32FC1 );
	cvmSet( M1, 0, 0, camera.getCameraHorizonX() );
	cvmSet( M1, 0, 1, camera.getCameraHorizonY() );
	cvmSet( M1, 0, 2, camera.getCameraHorizonZ() );
	cvmSet( M1, 1, 0, camera.getCameraVerticalX() );
	cvmSet( M1, 1, 1, camera.getCameraVerticalY() );
	cvmSet( M1, 1, 2, camera.getCameraVerticalZ() );
	cvmSet( M1, 2, 0, camera.getCameraAxisX() );
	cvmSet( M1, 2, 1, camera.getCameraAxisY() );
	cvmSet( M1, 2, 2, camera.getCameraAxisZ() );

	CvMat * M2 = cvCreateMat( 3, 4, CV_32FC1 );
	cvmSet( M2, 0, 0, 1.f );
	cvmSet( M2, 0, 1, 0.f );
	cvmSet( M2, 0, 2, 0.f );
	cvmSet( M2, 0, 3, -camera.getCameraPositionX() );
	cvmSet( M2, 1, 0, 0.f );
	cvmSet( M2, 1, 1, 1.f );
	cvmSet( M2, 1, 2, 0.f );
	cvmSet( M2, 1, 3, -camera.getCameraPositionY() );
	cvmSet( M2, 2, 0, 0.f );
	cvmSet( M2, 2, 1, 0.f );
	cvmSet( M2, 2, 2, 1.f );
	cvmSet( M2, 2, 3, -camera.getCameraPositionZ() );

	CvMat * M3 = cvCreateMat( 4, 1, CV_32FC1 );
	cvmSet( M3, 0, 0, posIn3D.getX() );
	cvmSet( M3, 1, 0, posIn3D.getY() );
	cvmSet( M3, 2, 0, posIn3D.getZ() );
	cvmSet( M3, 3, 0, 1.f );

	CvMat * Mtmp1 = cvCreateMat( 3, 1, CV_32FC1 );
	CvMat * Mtmp2 = cvCreateMat( 3, 1, CV_32FC1 );

	cvMatMul( M2, M3, Mtmp1 );
	cvMatMul( M1, Mtmp1, Mtmp2 );

	const float x = cvmGet( Mtmp2, 0, 0);
	const float y = cvmGet( Mtmp2, 1, 0);
	const float z = cvmGet( Mtmp2, 2, 0);

	cvReleaseMat( &M1 );
	cvReleaseMat( &M2 );
	cvReleaseMat( &M3 );
	cvReleaseMat( &Mtmp1 );
	cvReleaseMat( &Mtmp2 );

	return Vector3f( x, y, z );
}

list< Point2D > ModelReconstructor::getVoodooFeaturePointsOnOneFrame( FrameNumber frameNumber ) {
	list< Point2D > points;
	for( size_t idx = 0; idx < voodooLoader_->getNumberOfFeaturePoints(); ++idx ) {
		FeaturePoint fpoint = voodooLoader_->getFeaturePoint( idx );
		FeaturePoint::Points2DIterator point = fpoint.findPoint2D( frameNumber );
		const bool foundOnTheFrame = (point != fpoint.endPoint2D());
		if( !foundOnTheFrame ) continue;

		points.push_back( point->second );
	}
	return points;
}

list< Point2D > ModelReconstructor::getUserAddedFeaturePointsOnOneFrame( FrameNumber frameNumber ) {
	list< Point2D > points;
	FeaturePointsIterator iter = featurePoints_.begin();
	for(; iter != featurePoints_.end(); ++iter ) {
		FeaturePoint::Points2DIterator point = (*iter)->findPoint2D( frameNumber );
		const bool foundOnTheFrame = (point != (*iter)->endPoint2D());
		if( !foundOnTheFrame ) continue;

		points.push_back( point->second );
	}
	return points;
}

Point2D ModelReconstructor::getProjectionPoint( VoodooCameraInfo camera, Vector3f point3D ) {
	Vector3f posIn2D = getProjectionPosition( camera, point3D );
	assert( 0.f != posIn2D.getZ() );

	return Point2D( posIn2D.getX() / posIn2D.getZ() + camera.getImageSizeWidth() /2 -1,
			posIn2D.getY() / posIn2D.getZ() + camera.getImageSizeHeight() /2 -1);
}

list< Point2D > ModelReconstructor::getPotentialPointsOnOneFrame( FrameNumber frameNumber ) {
	if( NULL == voodooLoader_ || frameNumber >= voodooLoader_->getNumberOfCameraInfo() ) return PointsOnFrame();

	VoodooCameraInfo camera = voodooLoader_->getCameraInfo( frameNumber );

	list< Point2D > points;
	FeaturePointsIterator iter = featurePoints_.begin();
	for(; iter != featurePoints_.end(); ++iter ) {
		FeaturePoint::Points2DIterator point = (*iter)->findPoint2D( frameNumber );
		if( point != (*iter)->endPoint2D() ) continue;

		points.push_back( getProjectionPoint(camera, (*iter)->getPoint3D()) );
	}
	return points;
}

Point2D ModelReconstructor::getProjectionPoint( FrameNumber frameNumber, FeaturePoint * featurePoint ) {
	if( NULL == featurePoint ) return Point2D(0,0);

	FeaturePoint::Points2DIterator iterPoint = featurePoint->findPoint2D( frameNumber );
	if( iterPoint != featurePoint->endPoint2D() )
		return iterPoint->second;

	if( NULL == voodooLoader_ || frameNumber >= voodooLoader_->getNumberOfCameraInfo() ) return Point2D(0,0);

	VoodooCameraInfo camera = voodooLoader_->getCameraInfo( frameNumber );
	return getProjectionPoint( camera, featurePoint->getPoint3D() );
}

ModelReconstructor::LinesOnFrame ModelReconstructor::getLinesOnOneFrame( FrameNumber frameNumber ) {
	if( NULL == voodooLoader_ || frameNumber >= voodooLoader_->getNumberOfCameraInfo() ) return LinesOnFrame();;

	LinesOnFrame lines;
	FeatureLinesIterator iter = featureLines_.begin();
	for(; iter != featureLines_.end(); ++iter ) {
		FeaturePoint * const lineFrom = iter->getLineFrom();
		FeaturePoint * const lineTo = iter->getLineTo();

		lines.push_back( Line2D(
				getProjectionPoint( frameNumber, lineFrom ),
				getProjectionPoint( frameNumber, lineTo ) ) );
	}
	return lines;
}

ModelReconstructor::FeaturePointsForTriangle ModelReconstructor::findClosingFeaturePointsForTriangle( FeatureLine line1 ) {
	FeaturePointsForTriangle points;

	for(FeatureLinesIterator iterLine2 = featureLines_.begin(); iterLine2 != featureLines_.end(); ++iterLine2 ) {
		for(FeatureLinesIterator iterLine3 = featureLines_.begin(); iterLine3 != featureLines_.end(); ++iterLine3 ) {
			if( FeatureLine::isTriangle( line1, *iterLine2, *iterLine3 ) )
				points.insert( FeatureLine::getSharedPoint( *iterLine2, *iterLine3 ) );
		}
	}

	return points;
}

void ModelReconstructor::saveReconstructedModelOnFile( string filename ) {
	ofstream outfile;
	outfile.open( filename.c_str() );
	if( !outfile ) return;
	outfile << Impl::startingOfX3D;

	FeatureLinesIterator iterLine = featureLines_.begin();
	for(; iterLine != featureLines_.end(); ++iterLine ) {
		FeatureLinesIterator iterNextLine = iterLine;
		++iterNextLine;
		FeaturePointsForTriangle thirdPoints = findClosingFeaturePointsForTriangle( *iterLine );
		if( thirdPoints.empty() ) continue;

		FeaturePoint * const lineFrom = iterLine->getLineFrom();
		FeaturePoint * const lineTo = iterLine->getLineTo();

		FeaturePointsForTriangleIterator iterThirdPoint = thirdPoints.begin();
		for(; iterThirdPoint != thirdPoints.end(); ++iterThirdPoint) {
			outfile << endl;
			outfile << "<Transform><Shape>\
				<IndexedFaceSet coordIndex='0 1 2 -1' solid='false'>\
				  <Coordinate point='";
			outfile << lineFrom->getX() << " " << lineFrom->getY() << " " << lineFrom->getZ() << "    ";
			outfile << lineTo->getX() << " " << lineTo->getY() << " " << lineTo->getZ() << "    ";
			outfile << (*iterThirdPoint)->getX() << " " << (*iterThirdPoint)->getY() << " " << (*iterThirdPoint)->getZ();
			outfile << "'/>\
				</IndexedFaceSet>\
			  </Shape></Transform>" << endl;
		}
	}

	outfile << Impl::endingOfX3D;
}

string ModelReconstructor::Impl::startingOfX3D = "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\
<!DOCTYPE X3D PUBLIC \"ISO//Web3D//DTD X3D 3.0//EN\" \"http://www.web3d.org/specifications/x3d-3.0.dtd\">\
<X3D profile='Interactive' version='3.0' xmlns:xsd='http://www.w3.org/2001/XMLSchema-instance' xsd:noNamespaceSchemaLocation='http://www.web3d.org/specifications/x3d-3.0.xsd'>\
  <Scene>\
    <Transform scale='2 2 2'>\
      <Inline DEF='CoordinateAxes' url='\"http://www.web3d.org/x3d/content/examples/Basic/course/CoordinateAxes.x3d\" \"http://www.web3d.org/x3d/content/examples/Basic/course/CoordinateAxes.wrl\" \"https://savage.nps.edu/Savage/Tools/Authoring/CoordinateAxes.x3d\" \"https://savage.nps.edu/Savage/Tools/Authoring/CoordinateAxes.wrl\"'/>\
    </Transform>\
\
    <Transform scale='0.1 0.1 0.1' translation='0 0 5' rotation='1 0 0 3.14'>\
      <Shape>\
        <Appearance DEF='Cammi'>\
          <Material diffuseColor='0.45 0.55 0.55'/>\
        </Appearance>\
      </Shape>\
";

string ModelReconstructor::Impl::endingOfX3D = "    </Transform>\
  </Scene>\
</X3D>";

}
